import numpy as np
import cv2 as cv
import math
from scipy.spatial.transform import Rotation as R
<<<<<<< HEAD
import ArmControl

list_name = ["chenpi","hongzao","jiangtang","puer","qingju","baixiangguo"]
list_count = [2,2,2,2,2,2]
=======
import CamSettings as cam
import ArmControl

list_name = ["chenpi","hongzao","jiangtang","puer","qingju","meigui"]
list_count = [3,2,2,2,2,2]
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47

# 将接收数据转换为机械臂坐标值
def DataTransform(xmin,ymin,xmax,ymax):
    x_center=(xmin+xmax)/2
    y_center=(ymin+ymax)/2
    # point_x = np.int(x_center * 640)
    # point_y = np.int(y_center * 480)
    (a, b) = (round(((x_center - 320) / 4000), 5), round(((480 - y_center) / 3000) * 0.8 + 0.19, 5))
    return [a,b,x_center,y_center]

# 判断接收数据是否为上一帧数据的重复
def DataJudge(list,lastData):
    xmin = int(list[2])
    ymin = int(list[3])
    if int(lastData[2])- 10 <xmin< int(lastData[2]) + 10 and int(lastData[3]) - 10 <xmin< int(lastData[3]) + 10:
        return False
    if int(lastData[2]) == 0 and int(lastData[3]) == 0:
        return True
    else:
        return True

# 计算机械臂抓取数量
def DataCount(name):
    i = 0
    for i in range(len(list_name)):
        if name==list_name[i]:
            count = list_count[i]
        i+=1
    return count

def reverse(flag):
    if flag == 1:
        flag = 0
    elif flag == 0:
        flag = 1
    return flag

# 根据面积和深度计算物体重量s
def cal_weight(name,distance,square):
    k = 0.0322435022032315 * pow(distance, -2.08470228284519)
    x = -1.43767855406858 + (square / k) * 0.00105166747113723
    if name == "hongzao":
        weight = x
    if name == "juhua":
        weight = 3 * (square / k) / 6500
    return weight


# 将不同食材的抽屉拉开
def OpenLocation(name):
    ArmControl.Open_Box(name,list_name)


# 将不同食材的抽屉关闭
def CloseLocation(name):
    ArmControl.Close_Box(name,list_name)

# 旋转矩阵转换为变换矩阵
def matrix_transform(rm, x, y, z):
    cols = np.matrix([[x], [y], [z]])
    rm = np.c_[rm, cols]
    rows = np.matrix([0, 0, 0, 1])
    rm = np.r_[rm, rows]
    return rm

# 坐标转换
def axis_transform(x1,y1,z1,x2,y2,z2,Rq):
    # 欧拉角转旋转矩阵
    r1 = R.from_euler('xyz', [0, 0, 0], degrees=True)
    rm1 = r1.as_matrix()
    # 四元数转旋转矩阵
    r2 = R.from_quat(Rq)
    rm2 = r2.as_matrix()
    # 得到两个变换矩阵
    rm3 = matrix_transform(rm1, x1, y1, z1)
    rm4 = matrix_transform(rm2, x2, y2, z2)
    # 矩阵相乘
    rm5 = rm4 * rm3
    # 截取出旋转矩阵
    rm6 = rm5[0:3, 0:3]
    # 截取出xyz
    fx = rm5[0, 3]
    fy = rm5[1, 3]
    fz = rm5[2, 3]
    # 旋转矩阵转换为四元数
    r3 = R.from_matrix(rm6)
    qua = r3.as_quat()
    rm7 = np.append([fx,fy,fz], qua)
    return rm7

<<<<<<< HEAD
=======









>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
